"""
@file: dijkstra.py
@breif: Dijkstra motion planning
@author: Yang Haodong, Wu Maojia
@update: 2024.6.23
"""
import heapq

from .a_star import AStar
from python_motion_planning.utils import Env


class Dijkstra(AStar):
    def __init__(self, start: tuple, goal: tuple, env: Env, heuristic_type: str = "euclidean") -> None:
        super().__init__(start, goal, env, heuristic_type)
    
    def __str__(self) -> str:
        return "Dijkstra"

    def plan(self) -> tuple:
        """
        Class for Dijkstra motion planning.

        Parameters:
            start (tuple): start point coordinate
            goal (tuple): goal point coordinate
            env (Env): environment
            heuristic_type (str): heuristic function type

        Examples:
            >>> import python_motion_planning as pmp
            >>> planner = pmp.Dijkstra((5, 5), (45, 25), pmp.Grid(51, 31))
            >>> cost, path, expand = planner.plan()     # planning results only
            >>> planner.plot.animation(path, str(planner), cost, expand)  # animation
            >>> planner.run()       # run both planning and animation
        """
        # OPEN list (priority queue) and CLOSED list (hash table)
        OPEN = []
        heapq.heappush(OPEN, self.start)
        CLOSED = dict()

        while OPEN:
            node = heapq.heappop(OPEN)

            # exists in CLOSED list
            if node.current in CLOSED:
                continue

            # goal found
            if node == self.goal:
                CLOSED[node.current] = node
                cost, path = self.extractPath(CLOSED)
                return cost, path, list(CLOSED.values())

            for node_n in self.getNeighbor(node):
             
                # hit the obstacle
                if node_n.current in self.obstacles:
                    continue
                
                # exists in CLOSED list
                if node_n.current in CLOSED:
                    continue
                
                node_n.parent = node.current
                node_n.h = 0

                # goal found
                if node_n == self.goal:
                    heapq.heappush(OPEN, node_n)
                    break
                
                # update OPEN set
                heapq.heappush(OPEN, node_n)

            CLOSED[node.current] = node
        return [], [], []